#include "WPILib.h"
#include "EncoderSpeedController.h"
#include "DriverStationLCD.h"
#include "CameraColorTracker.h"

/**
 * This is a demo program showing the use of the RobotBase class.
 * The SimpleRobot class is the base of a robot application that will automatically call your
 * Autonomous and OperatorControl methods at the right time as controlled by the switches on
 * the driver station or the field controls.
 */

#define ACCELEROMETER_ZERO 1.5 // minimum voltage on accelerometer: 1.5 V at freefall
#define ACCELEROMETER_V_PER_G 0.3

#define MIN_PARTICLE_TO_IMAGE_PERCENT 0.25		// target is too small
#define MAX_PARTICLE_TO_IMAGE_PERCENT 100.0		// target is too close
#define SPEED_REDUCTION 2

//Controls 
//Driver Station Controls
#define INTAKE_POT_ANALOG_PORT 1

#define INTAKE_SWITCH_DIGITAL_PORT 1
#define ALLIANCE_SWITCH_DIGITAL_PORT 3

#define COMPRESSOR_SWITCH_DIGITAL_PORT 5
//#define KILL_SWITCH_DIGITAL_PORT 4 // disables all non-drive motors
//Driver Joystick Controls
#define SPEED_REDUCTION_BUTTON 1
#define DRIVER_BELT_REVERSE_BUTTON 2
#define LAUNCHER_SWITCH_DIGITAL_PORT 3
//#define CAMERA_TRACKING_BUTTON 3

class RobotDemo : public SimpleRobot {
	Encoder leftEncoder;
	Encoder rightEncoder;
	//Encoder launchEncoder;
	//Encoder intakeEncoder;
	Encoder turretEncoder;
	Ultrasonic pingSensor;
	Jaguar leftMotor;
	Jaguar rightMotor;
	Jaguar turretMotor;
	Jaguar launchMotor;
	Victor intakeMotor;

	DigitalInput leftSwitch;
	DigitalInput rightSwitch;

	Solenoid stopperSolenoid;
	//Relay compressorRelay;
	Compressor compressor;

	DriverStationLCD *dsLCD;

	EncoderSpeedController leftController;
	EncoderSpeedController rightController;
	//EncoderSpeedController launchController;
	RobotDrive myRobot; // robot drive system
	Joystick leftStick; // only joystick
	Joystick rightStick;
	Joystick auxStick; // auxiliary driver's joystick
	Accelerometer xAccel, yAccel;

	CameraColorTracker tracker;

public:
	RobotDemo(void) :
		leftEncoder(1, 2, true), rightEncoder(3, 4),
				//launchEncoder(5,6),
				turretEncoder(7, 8),
				pingSensor(9, 10),
				leftMotor(1),
				rightMotor(2),
				turretMotor(3),
				launchMotor(4),
				intakeMotor(6),
				leftSwitch(12), //limit switches for turret
				rightSwitch(13),
				stopperSolenoid(1), 
				//compressorRelay(1, Relay::kForwardOnly),
				compressor(11,1),

				leftController(&leftMotor, &leftEncoder), 
				rightController(&rightMotor, &rightEncoder),
				//launchController(&launchMotor, &launchEncoder),
				//myRobot(&leftController, &rightController),	// these must be initialized in the same order
				myRobot(&leftMotor, &rightMotor), leftStick(1), // as they are declared above.
				rightStick(2), auxStick(3), xAccel(1), yAccel(2) {
		dsLCD = DriverStationLCD::GetInstance();
		leftEncoder.Start();
		rightEncoder.Start();
		//launchEncoder.Start();
		turretEncoder.Start();
		pingSensor.SetAutomaticMode(true);
		GetWatchdog().SetExpiration(3000); // QUESTION:100 ms expiration time; aren't we using a 500 ms delay in operator control?
		StartCameraTask(10, 0, k320x240, ROT_180); // 10 fps
	}

	/**
	 * Drive left & right motors for 2 seconds then stop
	 */
	void RotateTurret(float rotateAmount) {
//		if ((leftSwitch.Get() == 1) && (rotateAmount > 0)) {
//			//TODO:stop rotation in negative direction (counterclockwise)
//			turretMotor.Set(0.0);
//			turretEncoder.Reset();
//		} else if ((rightSwitch.Get() == 1) && (rotateAmount < 0)) {
//			//TODO:stop rotation in positive direction (clockwise)
//			turretMotor.Set(0.0);
//		} else {
			turretMotor.Set(-1*rotateAmount);
//		}
	}
	void RotateTurretToSpot(float spot) {
		
	}
	void Autonomous(void) {
		GetWatchdog().SetEnabled(false);
		compressor.Start();
		//TODO: use a switch located on robot to determine team color (for camera tracking)
		//TODO: decide on strategy:
		//	1) Just drive around semi-randomly to avoid opponent's balls
		// 	2) Chase oppononents, without launching balls
		// 	3) Try to fire upon opponents with balls stored up at beginning
		pingSensor.SetAutomaticMode(false);
		//bool isBacking = false;
		intakeMotor.Set(0.5);
		myRobot.Drive(-1, 0.0);
		Wait(3.0);
		myRobot.Drive(-1, 1);
		Wait(3.0);
		myRobot.Drive(0,0);
		while (IsAutonomous()) {
			Wait(0.05);
		}
		
//		while (IsAutonomous()) {
//			pingSensor.Ping();
//			
//			if (isBacking) {
//				if (pingSensor.GetRangeMM() > 1000)
//					isBacking = false;
//			} else {
//				if (pingSensor.GetRangeMM() < 10) {
//					//do  nothing
//				}
//				else if (pingSensor.GetRangeMM() < 500) {
//					isBacking = true;
//				}
//			}
//			if (isBacking) {
//				myRobot.Drive(-1, -0.8);
//			} else {
//				myRobot.Drive(1, 0.0);
//			}
//			Wait(0.05);
//		cout << pingSensor.GetRangeMM() << "\n";
//		//cout << pingSensor.GetRangeInches() << "\n";
//		}
		//		myRobot.Drive(0.5, 0.0); 	// drive forwards half speed
		//		Wait(15); 				//    for 2 seconds
		//intakeController.Set(0);
		//		myRobot.Drive(0.0, 0.0); 	// stop robot
	}

	/**
	 * Runs the motors with arcade steering. 
	 */
	void OperatorControl(void) {
		GetWatchdog().SetEnabled(true);
		float targetX;
		const char targetingBg[22] = "---------------------";
		char targetingString[22];
		float leftStickValue;
		float rightStickValue;

		while (IsOperatorControl()) {
			GetWatchdog().Feed();
			dsLCD->Printf(DriverStationLCD::kMain_Line6, 3, "!! Hold SELECT !!");
			dsLCD->Printf(DriverStationLCD::kUser_Line2, 1,
					"Current time: %7.1f", GetClock());
			dsLCD->Printf(DriverStationLCD::kUser_Line6, 8, "Moose");

			//pingSensor.Ping();
			//myRobot.ArcadeDrive(stick); // drive with arcade style (use right stick)
			
			
			if ((leftStick.GetRawButton(SPEED_REDUCTION_BUTTON) == 1)
					|| (rightStick.GetRawButton(SPEED_REDUCTION_BUTTON) == 1)) {
				leftStickValue = leftStick.GetY();
				rightStickValue = rightStick.GetY();
			} else {

				leftStickValue = leftStick.GetY()*(rightStick.GetZ()+1.0)/2.0;
				rightStickValue = rightStick.GetY()*(rightStick.GetZ()+1.0)/2.0;
			}

			myRobot.TankDrive(leftStickValue, rightStickValue);


//			if (!auxStick.GetRawButton(2)) {
//				compressorRelay.Set(Relay::kOn);
//			} else {
//				compressorRelay.Set(Relay::kOff);
//			}
			//DONE: replace this section with a Compressor object
			if (m_ds->GetDigitalIn(COMPRESSOR_SWITCH_DIGITAL_PORT) == 1) {
				compressor.Start();
			} else {
				compressor.Stop();
			}
			
			
			// Switch controlling intake belt direction
			if ((m_ds->GetDigitalIn(INTAKE_SWITCH_DIGITAL_PORT) == 1) && (leftStick.GetRawButton(DRIVER_BELT_REVERSE_BUTTON) == 0)) {
				// Drive intake motor forward
				intakeMotor.Set(float(m_ds->GetAnalogIn(INTAKE_POT_ANALOG_PORT)) / 1024);
				//cout << "Intake Motor Forward";
			} else {
				// Drive intake motor in reverse
				intakeMotor.Set(float(m_ds->GetAnalogIn(INTAKE_POT_ANALOG_PORT)) / -1024); // Reverse direction if switch is flipped
				//cout << "Intake Motor Reverse";
			}
			
			if ((m_ds->GetDigitalIn(LAUNCHER_SWITCH_DIGITAL_PORT) == 1)) {
				//Drive Launcher motor forward
				launchMotor.Set((auxStick.GetZ() - 1.0) / 2.0);
			} else {
				//Drive launcher motor in reverse
				if (((auxStick.GetZ() - 1.0) / 2.0) < -0.5) {
					launchMotor.Set(((auxStick.GetZ() - 1.0) / -4.0));
				}
			}
			
			// Switch between manual or camera control of turret
			//			if (m_ds->GetDigitalIn(AIMING_SWITCH_DIGITAL_PORT) == 1) {
			//				//use camera targeting to aim turret
			//				//TODO: put in CameraColorTracker code
			//			}
			//			else {
			//				//use auxiliary joystick to control direction of turret
			//				turretMotor.Set(auxStick.GetX());
			//			}
			//			
			// TODO: put in code to stop turret rotation at turning limits

//			Image* cameraImage = frcCreateImage(IMAQ_IMAGE_HSL);
//			GetImage(cameraImage, NULL);
//			dsLCD->Printf(DriverStationLCD::kUser_Line3, 1,
//					"Visible targets: %i  ", tracker.FindTargets(cameraImage,
//							m_ds->GetDigitalIn(ALLIANCE_SWITCH_DIGITAL_PORT)));
//			targetX = tracker.FindBestTargetX();
//			strncpy(targetingString, targetingBg, 22);
//			if (targetX >= -1.0) {
//				targetingString[int((targetX + 1) * 10)] = '0';
//			}
//			dsLCD->Printf(DriverStationLCD::kUser_Line4, 1, targetingString);
//			frcDispose(cameraImage);

//			if (auxStick.GetRawButton(CAMERA_TRACKING_BUTTON) == 1) {
//				RotateTurret(tracker.FindBestTargetX());
//			} else {
//				RotateTurret(auxStick.GetX());
//			}
			RotateTurret(auxStick.GetX());
			//cout << leftSwitch.Get() << rightSwitch.Get() << "\n";
			// use thumbwheel of auxiliary joystick to control launch distance
			//launchController.Set(auxStick.GetZ()); 
			
			
			//TODO: switch this control to a potentiometer rather than the joystick thumbwheel(?)
			//TODO: use as encoder object to control acceleration of motor

			//TODO: put in control of stopper (pneumatic ram)
			// For now, we'll just use a depressed trigger to "hold" the stopper open
			// 		this will continually launch balls
			// Question: What is the easiest way to monitor for change of trigger, rather than current state?
			// 		this would allow us to close/open the ram without expending air when it is stationary
			//TODO: activate an LED on control board when stopper is open
			stopperSolenoid.Set(auxStick.GetTrigger());

			//cout << "Ping Distance:" << pingSensor.GetRangeMM() << " " << pingSensor.GetRangeInches() << "\n";
			//cout << "X Accel: " << xAccel.GetAcceleration() << "\n";
			//testController.Set(0.05);
			dsLCD->UpdateLCD();
			Wait(0.05);
			//cout << "Speeds: " << leftEncoder.GetPeriod() << " " << rightEncoder.GetPeriod() << "\n";
		}
	}
};

START_ROBOT_CLASS(RobotDemo)
;

